#include "ft_types.h"
#include "cache.h"
#include "aarch32.h"
#include "cp15.h"

#define IRQ_FIQ_MASK 0xC0U /* Mask IRQ and FIQ interrupts in cpsr */
#define SELECT_D_CACHE 0

extern s32 _svc_stack_end;
extern s32 _fiq_stack_start;

static inline u32 FCacheIcacheLineSize(void)
{
    u32 ctr;
    asm volatile("mrc p15, 0, %0, c0, c0, 1"
                 : "=r"(ctr));
    return 4 << (ctr & 0xF);
}

static inline u32 FCacheDcacheLineSize(void)
{
    u32 ctr;
    asm volatile("mrc p15, 0, %0, c0, c0, 1"
                 : "=r"(ctr));
    return 4 << ((ctr >> 16) & 0xF);
}

/*  Dcache */

void FCacheDCacheEnable(void)
{

    u32 ctrl_reg;

    ctrl_reg = MFCP(FREG_CP15_SYS_CONTROL);
    /* enable caches only if they are disabled */
    if ((ctrl_reg & FREG_CONTROL_DCACHE_BIT) == 0X00000000U)
    {

        /* invalidate the Data cache */
        FCacheDCacheInvalidate();

        ctrl_reg |= FREG_CONTROL_DCACHE_BIT;

        /* enable the Data cache */
        MTCP(FREG_CP15_SYS_CONTROL, ctrl_reg);
    }
}

void FCacheDCacheDisable(void)
{
    u32 ctrl_reg;
    /* clean and invalidate the Data cache */
    FCacheDCacheFlush();
    ctrl_reg = MFCP(FREG_CP15_SYS_CONTROL);

    ctrl_reg &= ~(FREG_CONTROL_DCACHE_BIT);
    /* disable the Data cache */
    MTCP(FREG_CP15_SYS_CONTROL, ctrl_reg);
}

void FCacheDCacheFlushLine(u32 adr)
{
    u32 currmask;

    currmask = MFCPSR();
    MTCPSR(currmask | IRQ_FIQ_MASK);

    /* Select cache level 0 and D cache in CSSR */
    MTCP(FREG_CP15_CACHE_SIZE_SEL, 0x0);
    ISB();
    MTCP(FREG_CP15_CLEAN_INVAL_DC_LINE_MVA_POC, (adr & (~0x3F)));
    /* Wait for invalidate to complete */
    DSB();
    /* Select cache level 1 and D cache in CSSR */
    MTCP(FREG_CP15_CACHE_SIZE_SEL, 0x2);
    ISB();
    MTCP(FREG_CP15_CLEAN_INVAL_DC_LINE_MVA_POC, (adr & (~0x3F)));
    /* Wait for invalidate to complete */
    DSB();
    MTCPSR(currmask);
}

void FCacheDCacheInvalidate(void)
{
    register u32 csid_reg;
    register u32 c7_reg;
    u32 line_size;
    u32 num_ways;
    u32 way;
    u32 way_index;
    u32 way_adjust;
    u32 set;
    u32 set_index;
    u32 num_set;
    u32 num_cache_level;
    u32 cache_level;
    u32 currmask;
    u32 stack_start;
    u32 stack_end;
    u32 stack_size;

    stack_end = (u32)&_svc_stack_end;
    stack_start = (u32)&_fiq_stack_start;
    stack_size = stack_start - stack_end;

    /*Flush stack memory to save return address */
    FCacheDCacheFlushRange(stack_end, stack_size);

    currmask = MFCPSR();
    MTCPSR(currmask | IRQ_FIQ_MASK);

    /* Number of level of cache */
    num_cache_level = (MFCP(FREG_CP15_CACHE_LEVEL_ID) >> 24U) & 0x00000007U;

    for (cache_level = 0U, way_adjust = 0x1E; cache_level < num_cache_level;
         cache_level++, way_adjust = way_adjust - 2)
    {

        MTCP(FREG_CP15_CACHE_SIZE_SEL, ((cache_level << 1) | SELECT_D_CACHE));
        ISB();

        csid_reg = MFCP(FREG_CP15_CACHE_SIZE_ID);

        /* Get the cacheline size, way size, index size from csidr */
        line_size = (csid_reg & 0x00000007U) + 0x00000004U;

        /* Number of ways */
        num_ways = (csid_reg & 0x00001FFFU) >> 3U;
        num_ways += 0X00000001U;

        /*Number of set */
        num_set = (csid_reg >> 13U) & 0x00007FFFU;
        num_set += 0X00000001U;

        way = 0U;
        set = 0U;

        /* Invalidate all the cachelines */
        for (way_index = 0U; way_index < num_ways; way_index++)
        {
            for (set_index = 0U; set_index < num_set; set_index++)
            {
                c7_reg = way | set | (cache_level << 1);
                MTCP(FREG_CP15_INVAL_DC_LINE_SW, c7_reg);
                set += (0x00000001U << line_size);
            }
            set = 0U;
            way += (0x00000001U << way_adjust);
        }

        /* Wait for invalidate to complete */
        DSB();
    }
    MTCPSR(currmask);
}

void FCacheDCacheInvalidateRange(uintptr adr, u32 len)
{
    const u32 cacheline = FCacheDcacheLineSize();
    u32 end;
    u32 tempadr = adr;
    u32 tempend;
    u32 currmask;

    currmask = MFCPSR();
    MTCPSR(currmask | IRQ_FIQ_MASK);
    if (len != 0U)
    {
        end = tempadr + len;
        tempend = end;

        if ((tempadr & (cacheline - 1U)) != 0U)
        {
            tempadr &= (~(cacheline - 1U));
            FCacheDCacheFlushLine(tempadr);
            tempadr += cacheline;
        }
        if ((tempend & (cacheline - 1U)) != 0U)
        {
            tempend &= (~(cacheline - 1U));
            FCacheDCacheFlushLine(tempend);
        }

        while (tempadr < tempend)
        {
            /* Select cache level 0 and D cache in CSSR */
            MTCP(FREG_CP15_CACHE_SIZE_SEL, 0x0);
            /* Invalidate Data cache line */
            MTCP(FREG_CP15_INVAL_DC_LINE_MVA_POC,
                 (tempadr & (~0x3F)));
            /* Wait for invalidate to complete */
            DSB();
            /* Select cache level 0 and D cache in CSSR */
            MTCP(FREG_CP15_CACHE_SIZE_SEL, 0x2);
            /* Invalidate Data cache line */
            MTCP(FREG_CP15_INVAL_DC_LINE_MVA_POC,
                 (tempadr & (~0x3F)));
            /* Wait for invalidate to complete */
            DSB();
            tempadr += cacheline;
        }
    }
    MTCPSR(currmask);
}

void FCacheDCacheFlush(void)
{
    register u32 csid_reg;
    register u32 c7_reg;
    u32 line_size;
    u32 num_ways;
    u32 way;
    u32 way_index;
    u32 way_adjust;
    u32 set;
    u32 set_index;
    u32 num_set;
    u32 num_cache_level;
    u32 cache_level;
    u32 currmask;

    currmask = MFCPSR();
    MTCPSR(currmask | IRQ_FIQ_MASK);

    /* Number of level of cache*/
    num_cache_level = (MFCP(FREG_CP15_CACHE_LEVEL_ID) >> 24U) & 0x00000007U;

    for (cache_level = 0U, way_adjust = 0x1E; cache_level < num_cache_level;
         cache_level++, way_adjust = way_adjust - 2)
    {
        /* Select cache level  and D cache in CSSELR */
        MTCP(FREG_CP15_CACHE_SIZE_SEL, ((cache_level << 1) | SELECT_D_CACHE));
        ISB();

        csid_reg = MFCP(FREG_CP15_CACHE_SIZE_ID);

        /* Get the cacheline size, way size, index size from csidr */
        line_size = (csid_reg & 0x00000007U) + 0x00000004U;

        /* Number of ways */
        num_ways = (csid_reg & 0x00001FFFU) >> 3U;
        num_ways += 0X00000001U;

        /*Number of set*/
        num_set = (csid_reg >> 13U) & 0x00007FFFU;
        num_set += 0X00000001U;

        way = 0U;
        set = 0U;

        /* Invalidate all the cachelines */
        for (way_index = 0U; way_index < num_ways; way_index++)
        {
            for (set_index = 0U; set_index < num_set; set_index++)
            {
                c7_reg = way | set | (cache_level << 1);
                MTCP(FREG_CP15_CLEAN_INVAL_DC_LINE_SW, c7_reg);
                set += (0x00000001U << line_size);
            }
            set = 0U;
            way += (0x00000001U << way_adjust);
        }

        /* Wait for invalidate to complete */
        DSB();
    }
    MTCPSR(currmask);
}

void FCacheDCacheFlushRange(uintptr adr, u32 len)
{
    const u32 cacheline = FCacheDcacheLineSize();
    u32 end;
    u32 tempadr = adr;
    u32 tempend;
    u32 currmask;

    currmask = MFCPSR();
    MTCPSR(currmask | IRQ_FIQ_MASK);
    if (len != 0U)
    {
        end = tempadr + len;
        tempend = end;

        if ((tempadr & (cacheline - 1U)) != 0U)
        {
            tempadr &= (~(cacheline - 1U));
            FCacheDCacheFlushLine(tempadr);
            tempadr += cacheline;
        }
        if ((tempend & (cacheline - 1U)) != 0U)
        {
            tempend &= (~(cacheline - 1U));
            FCacheDCacheFlushLine(tempend);
        }

        while (tempadr < tempend)
        {
            /* Select cache level 0 and D cache in CSSR */
            MTCP(FREG_CP15_CACHE_SIZE_SEL, 0x0);
            /* Invalidate Data cache line */
            MTCP(FREG_CP15_CLEAN_INVAL_DC_LINE_MVA_POC,
                 (tempadr & (~0x3F)));
            /* Wait for invalidate to complete */
            DSB();
            /* Select cache level 0 and D cache in CSSR */
            MTCP(FREG_CP15_CACHE_SIZE_SEL, 0x2);
            /* Invalidate Data cache line */
            MTCP(FREG_CP15_CLEAN_INVAL_DC_LINE_MVA_POC,
                 (tempadr & (~0x3F)));
            /* Wait for invalidate to complete */
            DSB();
            tempadr += cacheline;
        }
    }
    MTCPSR(currmask);
}

/*  Icache */

void FCacheICacheEnable(void)
{
    u32 ctrl_reg;

    ctrl_reg = MFCP(FREG_CP15_SYS_CONTROL);
    /* enable caches only if they are disabled */
    if ((ctrl_reg & FREG_CONTROL_ICACHE_BIT) == 0x00000000U)
    {
        /* invalidate the instruction cache */
        FCacheICacheInvalidate();
        ctrl_reg |= FREG_CONTROL_ICACHE_BIT;
        /* enable the instruction cache */
        MTCP(FREG_CP15_SYS_CONTROL, ctrl_reg);
    }
}

void FCacheICacheDisable(void)
{
    u32 ctrl_reg;

    ctrl_reg = MFCP(FREG_CP15_SYS_CONTROL);
    /* invalidate the instruction cache */
    FCacheICacheInvalidate();
    ctrl_reg &= ~(FREG_CONTROL_ICACHE_BIT);
    /* disable the instruction cache */
    MTCP(FREG_CP15_SYS_CONTROL, ctrl_reg);
}

void FCacheICacheInvalidate(void)
{
    unsigned int currmask;

    currmask = MFCPSR();
    MTCPSR(currmask | IRQ_FIQ_MASK);
    MTCP(FREG_CP15_CACHE_SIZE_SEL, 0x1);
    DSB();
    /* invalidate the instruction cache */
    MTCP(FREG_CP15_INVAL_IC_POU, 0x0);
    /* Wait for invalidate to complete */
    DSB();
    MTCPSR(currmask);
}

void FCacheICacheInvalidateLine(u32 adr)
{
    u32 currmask;

    currmask = MFCPSR();
    MTCPSR(currmask | IRQ_FIQ_MASK);

    MTCP(FREG_CP15_CACHE_SIZE_SEL, 0x1);
    /*Invalidate I Cache line*/
    MTCP(FREG_CP15_INVAL_IC_LINE_MVA_POU, adr & (~0x3F));
    /* Wait for invalidate to complete */
    DSB();
    MTCPSR(currmask);
}

void FCacheICacheInvalidateRange(uintptr adr, u32 len)
{
    const u32 cacheline = FCacheIcacheLineSize();
    u32 end;
    u32 tempadr = adr;
    u32 tempend;
    u32 currmask;

    currmask = MFCPSR();
    MTCPSR(currmask | IRQ_FIQ_MASK);

    if (len != 0x00000000U)
    {
        end = tempadr + len;
        tempend = end;
        tempadr &= ~(cacheline - 0x00000001U);

        /* Select cache Level 0 I-cache in CSSR */
        MTCP(FREG_CP15_CACHE_SIZE_SEL, 0x1);
        while (tempadr < tempend)
        {
            /*Invalidate I Cache line*/
            MTCP(FREG_CP15_INVAL_IC_LINE_MVA_POU, adr & (~0x3F));

            tempadr += cacheline;
        }
    }
    /* Wait for invalidate to complete */
    DSB();
    MTCPSR(currmask);
}